#include "CStereoCamera.h"
#include "nvmath.h"
#include <iostream>

using std::cout;

CStereoCamera::CStereoCamera()
{

}

CStereoCamera::~CStereoCamera()
{

}

void CStereoCamera::GetRotateTranslation(Mat& leftCameraMatrix, Mat& leftDistCoeffs, 
					 Mat& rightCameraMatrix, Mat& rightDistCoeffs,
					 const vector<Point3f> &objectPoints,
					 const vector<Point2f>& leftImagePoints, const vector<Point2f>& rightImagePoints,
					 const CvSize &imageSize,
					 float &thetaX, float &thetaY, float &thetaZ, Mat &tanslation)
{
	assert(leftImagePoints.size() == rightImagePoints.size());
	Mat R, T, E, F;
	vector<vector<Point3f> > objP;
	vector<vector<Point2f> > leftP;
	vector<vector<Point2f> > rightP;
	objP.push_back(objectPoints);
	leftP.push_back(leftImagePoints);
	rightP.push_back(rightImagePoints);

	//computer extrinsic matrix
	cv::stereoCalibrate( objP,
					leftP,
					rightP,
					leftCameraMatrix, leftDistCoeffs,
					rightCameraMatrix,rightDistCoeffs,
					imageSize, R, T, E, F,
					TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6),
					CALIB_FIX_INTRINSIC);

	nv::matrix4f rot;
	for (int i = 0; i<3; ++i)
	{
		for (int j = 0; j<3; ++j)
		{
			rot(i,j) = R.at<double>(i, j);
		}
	}
	float theta1, theta2;
	float psi1, psi2;
	float phi1, phi2;
	if (rot._31 != 1.0 && rot._31 != -1.0 )
	{
		theta1 = -asin(rot._31);
		psi1 = atan2(rot._32/cos(theta1), rot._33/cos(theta1));
		phi1 = atan2(rot._21/cos(theta1), rot._11/cos(theta1));

		theta1 *= 180.0/CV_PI;
		psi1   *= 180.0/CV_PI;
		phi1   *= 180.0/CV_PI;
//		printf("angle = %f %f %f\n", theta1, psi1, phi1);
	}
	thetaX = theta1;
	thetaY = psi1;
	thetaZ = phi1;
	//translation
	nv::vec3f tras = nv::vec3f(T.at<double>(0, 0), T.at<double>(1, 0), T.at<double>(2, 0));
	//printf("translation = %f %f %f\n\n", tras.x, tras.y, tras.z);

	//computer reprojection error
	Mat rvec;
	Mat tvec;	
	vector<Point2f> reprojImgPts;
	double error = 0;
	cv::solvePnP(Mat(objectPoints), Mat(leftImagePoints), leftCameraMatrix, leftDistCoeffs, rvec, tvec);
	cv::projectPoints(Mat(objectPoints),rvec, tvec,	 leftCameraMatrix, leftDistCoeffs, reprojImgPts);
	for (int i = 0; i<reprojImgPts.size(); ++i)
	{
		Point2f p_dif = reprojImgPts[i] - leftImagePoints[i];
		error += sqrt(p_dif.x * p_dif.x + p_dif.y * p_dif.y);
	}
	printf("left reprojection error %f\n", error/leftImagePoints.size());

	error = 0;
	reprojImgPts.clear();
	cv::solvePnP(Mat(objectPoints), Mat(rightImagePoints), rightCameraMatrix, rightDistCoeffs, rvec, tvec);
	cv::projectPoints(Mat(objectPoints),rvec, tvec,	 rightCameraMatrix, rightDistCoeffs, reprojImgPts);
	for (int i = 0; i<reprojImgPts.size(); ++i)
	{
		Point2f p_dif = reprojImgPts[i] - rightImagePoints[i];
		error += sqrt(p_dif.x * p_dif.x + p_dif.y * p_dif.y);
	}
	printf("right reprojection error %f\n", error/rightImagePoints.size());
	tanslation = T;
	int a = 1;
}

double CStereoCamera::AlignTwoImages(Mat& leftCameraMatrix, Mat& leftDistCoeffs, 
			   Mat& rightCameraMatrix, Mat& rightDistCoeffs,
			   const vector<Point3f> &objectPoints,
			   const vector<Point2f>& leftImagePoints, 
			   const vector<Point2f>& rightImagePoints,
			   IplImage *leftImage, IplImage *rightImage,
			   IplImage *alignLeft, IplImage *alignRight, double alpha)
{
	assert(leftImagePoints.size() == rightImagePoints.size());
	assert(leftImage && rightImage && alignLeft && alignRight);

	double f[9];
	double r[9], t[3], e[9];
	Mat R = Mat(3, 3, CV_64F, r);
	Mat T = Mat(3, 1, CV_64F, t);
	Mat E = Mat(3, 3, CV_64F, e);
	Mat F = Mat(3, 3, CV_64F, f);
	vector<vector<Point3f> > objP;
	vector<vector<Point2f> > leftP;
	vector<vector<Point2f> > rightP;
	cv::Size imageSize = cv::Size(leftImage->width, leftImage->height);

	objP.push_back(objectPoints);
	leftP.push_back(leftImagePoints);
	rightP.push_back(rightImagePoints);

	//computer extrinsic matrix
	double error = cv::stereoCalibrate( objP,
						leftP,
						rightP,
						leftCameraMatrix, leftDistCoeffs,
						rightCameraMatrix,rightDistCoeffs,
						cvGetSize(leftImage), R, T, E, F,
						TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6),
						CALIB_FIX_INTRINSIC);
	//CALIB_FIX_PRINCIPAL_POINT

	// because the output fundamental matrix implicitly
	// includes all the output information,
	// we can check the quality of calibration using the
	// epipolar geometry constraint: m2^t*F*m1=0  Fm1 = I

	//evaluate calibration error using epipolar geometry constraint
	Mat leftPts  = Mat(leftImagePoints, true);
	Mat rightPts = Mat(rightImagePoints, true);

	cv::undistortPoints(leftPts, leftPts, leftCameraMatrix, leftDistCoeffs, Mat(), leftCameraMatrix);
	cv::undistortPoints(rightPts, rightPts, rightCameraMatrix, rightDistCoeffs, Mat(), rightCameraMatrix);

	vector<Vec3f> lines[2];
	cv::computeCorrespondEpilines(leftPts, 1, F, lines[0]);  // I = FX
	cv::computeCorrespondEpilines(rightPts, 2, F, lines[1]);

	double left_err  = 0;
	double right_err = 0;
	double err = 0.0;

	for (int i = 0; i<leftImagePoints.size(); ++i)
	{
		left_err  += fabs(leftImagePoints[i].x * lines[1][i][0] +
				   		  leftImagePoints[i].y * lines[1][i][1] + lines[1][i][2]);
		right_err += fabs(rightImagePoints[i].x * lines[0][i][0] +
						  rightImagePoints[i].y * lines[0][i][1] + lines[0][i][2]);						
	}
	err = left_err + right_err;
	//could evaluation the precision of calibration
//	printf("left error = %f\n right error = %f\n total error = %f\n", left_err, right_err, err);

	//rectify left image and right image
	double r1[9], r2[9], p1[12], p2[12]; 
	Mat R1 = Mat(3, 3, CV_64F, r1);
	Mat R2 = Mat(3, 3, CV_64F, r2);
	Mat P1 = Mat(3, 4, CV_64F, p1);
	Mat P2 = Mat(3, 4, CV_64F, p2);
	Mat Q;
	Rect validRoi[2];
	cv::stereoRectify(leftCameraMatrix, leftDistCoeffs,
					  rightCameraMatrix, rightDistCoeffs,
					  imageSize, R, T, R1, R2, P1, P2, Q,
					  CALIB_ZERO_DISPARITY, alpha, imageSize, &validRoi[0], &validRoi[1]);
	//
	Mat rmap[2][2];
	cv::initUndistortRectifyMap(leftCameraMatrix, leftDistCoeffs, R1, P1, imageSize, CV_16SC2, rmap[0][0], rmap[0][1]);
	cv::initUndistortRectifyMap(rightCameraMatrix, rightDistCoeffs, R2, P2, imageSize, CV_16SC2, rmap[1][0], rmap[1][1]);
	
	cv::remap(Mat(leftImage), Mat(alignLeft), rmap[0][0], rmap[0][1], CV_INTER_LINEAR);
	cv::remap(Mat(rightImage), Mat(alignRight), rmap[1][0], rmap[1][1], CV_INTER_LINEAR);

	return err;	
}

void CStereoCamera::AlignImageSequence(Mat& leftCameraMatrix, Mat& leftDistCoeffs, 
										Mat& rightCameraMatrix, Mat& rightDistCoeffs,
										const vector<Point3f> &objectPoints,
										const vector<vector<Point2f>>& leftImagePoints, 
										const vector<vector<Point2f>>& rightImagePoints,
										const CvSize &imgSize,
										double alpha, Mat rmap[][2])
{
	assert(leftImagePoints.size() == rightImagePoints.size());

	double f[9];
	double r[9], t[3], e[9];
	Mat R = Mat(3, 3, CV_64F, r);
	Mat T = Mat(3, 1, CV_64F, t);
	Mat E = Mat(3, 3, CV_64F, e);
	Mat F = Mat(3, 3, CV_64F, f);
	vector<vector<Point3f> > objP;

	objP.resize(leftImagePoints.size(), objectPoints);
	//computer extrinsic matrix
	double error = cv::stereoCalibrate( objP,
		leftImagePoints,
		rightImagePoints,
		leftCameraMatrix, leftDistCoeffs,
		rightCameraMatrix,rightDistCoeffs,
		imgSize, R, T, E, F,
		TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6),
		CALIB_FIX_INTRINSIC);
	//	printf("left error = %f\n right error = %f\n total error = %f\n", left_err, right_err, err);

	//rectify left image and right image
	double r1[9], r2[9], p1[12], p2[12]; 
	Mat R1 = Mat(3, 3, CV_64F, r1);
	Mat R2 = Mat(3, 3, CV_64F, r2);
	Mat P1 = Mat(3, 4, CV_64F, p1);
	Mat P2 = Mat(3, 4, CV_64F, p2);
	Mat Q;
	Rect validRoi[2];
	cv::stereoRectify(leftCameraMatrix, leftDistCoeffs,
						rightCameraMatrix, rightDistCoeffs,
						imgSize, R, T, R1, R2, P1, P2, Q,
						CALIB_ZERO_DISPARITY, alpha, cv::Size(imgSize), &validRoi[0], &validRoi[1]);
	//
	cv::initUndistortRectifyMap(leftCameraMatrix, leftDistCoeffs, R1, P1, imgSize, CV_16SC2, rmap[0][0], rmap[0][1]);
	cv::initUndistortRectifyMap(rightCameraMatrix, rightDistCoeffs, R2, P2, imgSize, CV_16SC2, rmap[1][0], rmap[1][1]);

}